package gridmapper.server;

import gridmapper.RobotData;
import gridmapper.RobotDataParser;
import gridmapper.RobotHeading;
import gridmapper.ServerData;
import gridmapper.ServerDataParser;
import gridmapper.server.planner.CellState;
import gridmapper.server.planner.CombinedMove;
import gridmapper.server.planner.GridPlanner;

import java.awt.Point;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.SortedSet;
import java.util.TreeSet;

import lejos.pc.comm.NXTCommFactory;
import lejos.pc.comm.NXTConnector;

public class GridMapperServer implements Runnable {

	public static final RobotInfo RheinoBot = new RobotInfo("RheinoBot",
			"001653090A7F", new Point(2, 0));
	public static final RobotInfo NXT = new RobotInfo("NXT", "0016530B13F1",
			new Point(3, 0));

	private final SortedSet<RobotInfo> robots;
	private Map<RobotInfo, NXTConnector> connections;
	private RobotDataParser rdp;
	private ServerDataParser sdp;
	private Map<RobotInfo, GridPlanner> planners;
	private List<CombinedMove> moves;

	/**
	 * @param args
	 */
	public static void main(String[] args) {
		GridMapperServer server = new GridMapperServer();
		new Thread(server).start();
	}

	public GridMapperServer() {
		robots = new TreeSet<RobotInfo>();
		robots.add(RheinoBot);
		robots.add(NXT);
		connections = new HashMap<RobotInfo, NXTConnector>();
		planners = new HashMap<RobotInfo, GridPlanner>();
	}

	private NXTConnector getConnector(RobotInfo info) {
		NXTConnector conn = new NXTConnector();
		if (conn.connectTo(info.name, info.address, NXTCommFactory.BLUETOOTH))
			return conn;
		return null;
	}

	private void connectRobots() {
		connections.clear();
		for (RobotInfo bot : robots) {
			System.out.println("Connecting to " + bot.toString());
			NXTConnector conn = getConnector(bot);
			if (conn == null) {
				System.err.println("Unable to connect to " + bot.toString());
				robots.remove(bot);
			} else
				connections.put(bot, conn);
		}
	}

	@Override
	public void run() {
		connectRobots();
		if (robots.size() == 0) {
			System.err.println("ERROR: Unable to connect to any robot.");
			System.exit(-1);
		}

		rdp = new RobotDataParser();
		sdp = new ServerDataParser();
		for (RobotInfo bot : robots)
			planners.put(bot, new GridPlanner(4, 6, bot.position,
					RobotHeading.NORTH));

		while (true) {
			Map<RobotInfo, RobotData> robotsStates = sense();
			if (robotsStates == null) {
				System.err
						.println("ERROR: Unable to collect data from any robot.");
				System.exit(-1);
			}
			Map<RobotInfo, ServerData> robotsInstructions = plan(robotsStates);
			if (!act(robotsInstructions)) {
				System.err
						.println("ERROR: Final instruction sent or unable to send any robot its instructions.");
				System.exit(-1);
			}
			for (RobotInfo bot : robots)
				drawGrid(bot);
		}
	}

	private boolean act(Map<RobotInfo, ServerData> robotsInstructions) {
		for (RobotInfo bot : robots) {
			ServerData data = robotsInstructions.get(bot);
			if (!sdp.writeData(connections.get(bot).getDataOut(), data)) {
				System.err.println("ERROR: Unable to send data to "
						+ bot.toString());
				for (GridPlanner planner : planners.values())
					planner.changeCellState(bot.position,
							CellState.scannedOccupied, false);
				robots.remove(bot);
			} else if (data.alive) {
				bot.position.setLocation(planners.get(bot)
						.convertHeadingToCoordinate(bot.position,
								data.robotHeading));
			}
		}

		boolean alive = true;
		for (ServerData data : robotsInstructions.values())
			if (!data.alive)
				alive = false;
		// for (RobotInfo bot : robots) {
		// drawGrid(bot);
		// }

		return alive && robots.size() != 0;
	}

	private void drawGrid(RobotInfo bot) {
		GridPlanner planner = planners.get(bot);
		System.out.println(bot.toString());
		for (int y = planner.cells.length - 1; y >= 0; --y) {
			System.out.print(y);
			for (int x = planner.cells[0].length - 1; x >= 0; --x) {
				char cellValue = 0;
				switch (planner.cells[y][x]) {
				case open:
					cellValue = '?';
					break;
				case explored:
					cellValue = 'X';
					break;
				case scannedOccupied:
					cellValue = 'O';
					break;
				case scannedOpen:
					cellValue = ' ';
					break;
				}

				System.out.print(' ');
				System.out.print(cellValue);
			}
			System.out.println();
		}
		System.out.print(' ');
		for (int x = planner.cells[0].length - 1; x >= 0; --x) {
			System.out.print(' ');
			System.out.print(x);
		}
		System.out.println();
		System.out.println();
	}

	private Map<RobotInfo, ServerData> plan(Map<RobotInfo, RobotData> robotState) {
		Map<RobotInfo, ServerData> retVal = new HashMap<RobotInfo, ServerData>();
		RobotInfo[] robotsArr = (RobotInfo[]) robots
				.toArray(new RobotInfo[robots.size()]);

		if (isReplanRequired(robotState)) {
			System.out.println("Replanning...");
			Map<RobotInfo, List<Point>> plans = new HashMap<RobotInfo, List<Point>>();

			for (RobotInfo bot : robots) {
				GridPlanner planner = planners.get(bot);
				planner.Robot1_Location = bot.position;
				planner.run();
				if (planner.moves == null || planner.moves.size() == 0) {
					planner.moves = new ArrayList<Point>();
					planner.moves.add(bot.position);
				}
				plans.put(bot, planner.moves);
			}

			RobotInfo botWithShortPlan = RheinoBot;
			for (RobotInfo bot : robots) {
				if (plans.get(bot).size() < plans.get(botWithShortPlan).size()) {
					botWithShortPlan = bot;
				}
			}

			Point botObstacle = plans.get(botWithShortPlan).get(
					plans.get(botWithShortPlan).size() - 1);
			for (RobotInfo bot : robots) {
				if (!bot.equals(botWithShortPlan)) {
					CellState original = planners.get(bot).cells[botObstacle.y][botObstacle.x];
					planners.get(bot).changeCellState(botObstacle,
							CellState.scannedOccupied, true);
					// drawGrid(bot);

					GridPlanner planner = planners.get(bot);
					planner.Robot1_Location = bot.position;
					planner.run();
					if (planner.moves == null || planner.moves.size() == 0) {
						planner.moves = new ArrayList<Point>();
						planner.moves.add(bot.position);
					}
					plans.put(bot, planner.moves);

					planners.get(bot).changeCellState(botObstacle, original,
							true);
					// drawGrid(bot);
				}
			}

			boolean done = true;
			for (RobotInfo bot : robots)
				if (!bot.position.equals(plans.get(bot).get(0)))
					done = false;

			moves = !done ? getCombinedMoves(plans) : null;
		}

		CombinedMove nextMove = moves != null && moves.size() > 0 ? moves
				.remove(0) : null;
		boolean alive = nextMove != null;
		if (alive) {
			System.err.println(nextMove);
			if (moves.size() > 0) {
				System.err.println(moves.get(moves.size() - 1));
			}
		}
		for (int i = 0; i < robots.size(); ++i) {
			int heading = RobotHeading.INVALID;
			if (alive)
				switch (i) {
				case 0:
					heading = planners.get(robotsArr[i])
							.convertCoordinateToHeading(robotsArr[i].position,
									nextMove.Robot1Move);
					break;
				case 1:
					heading = planners.get(robotsArr[i])
							.convertCoordinateToHeading(robotsArr[i].position,
									nextMove.Robot2Move);
				}
			System.err.println(robotsArr[i].toString() + '>'
					+ RobotHeading.toString(heading));
			retVal.put(robotsArr[i], new ServerData(heading, alive));
		}

		return retVal;
	}

	private List<CombinedMove> getCombinedMoves(
			Map<RobotInfo, List<Point>> plans) {
		while (addStalls(plans))
			;

		RobotInfo smallPath = RheinoBot;
		RobotInfo longPath = NXT;

		for (RobotInfo bot : robots) {
			if (plans.get(bot).size() < plans.get(smallPath).size()) {
				longPath = smallPath;
				smallPath = bot;
			}
		}

		while (plans.get(smallPath).size() < plans.get(longPath).size()) {
			plans.get(smallPath).add(
					plans.get(smallPath).get(plans.get(smallPath).size() - 1));
		}

		List<CombinedMove> combinedMoves = new ArrayList<CombinedMove>();
		for (int i = 0; i < plans.get(longPath).size(); ++i) {
			if (RheinoBot.equals(smallPath)) {
				combinedMoves.add(new CombinedMove(plans.get(smallPath).get(i),
						plans.get(longPath).get(i)));
			} else
				combinedMoves.add(new CombinedMove(plans.get(longPath).get(i),
						plans.get(smallPath).get(i)));
		}

		return combinedMoves;
	}

	private boolean addStalls(Map<RobotInfo, List<Point>> plans) {
		RobotInfo smallPath = RheinoBot;
		RobotInfo longPath = NXT;
		int minSize = plans.get(smallPath).size();

		for (RobotInfo bot : robots) {
			if (plans.get(bot).size() < minSize) {
				minSize = plans.get(bot).size();
				longPath = smallPath;
				smallPath = bot;
			}
		}
		// TODO I dont think the robots current position is in the list so this
		// needs some work
		for (int i = 0; i < plans.get(smallPath).size() - 1; ++i) {
			if (plans.get(longPath).subList(i, i + 2).containsAll(
					plans.get(smallPath).subList(i, i + 2))) {
				if (i > 0)
					plans.get(longPath).add(i - 1,
							plans.get(longPath).get(i - 1));
				else
					plans.get(longPath).add(0, longPath.position);
				return true;
			} else {
				for (Point p : plans.get(smallPath).subList(i, i + 2)) {
					if (plans.get(longPath).subList(i, i + 2).contains(p)) {
						plans.get(longPath).add(i, plans.get(longPath).get(i));
						return true;
					}
				}
			}
		}

		int i = plans.get(smallPath).size() - 1;
		if (i == 0
				&& plans.get(longPath).get(i).equals(
						plans.get(smallPath).get(i))) {
			plans.get(longPath).add(i, longPath.position);
			return true;
		} else if (i > 0
				&& plans.get(longPath).get(i).equals(
						plans.get(smallPath).get(i))) {
			plans.get(longPath).add(i, plans.get(longPath).get(i - 1));
			return true;
		}

		return false;
	}

	// private void drawPlan(List<CombinedMove> moves) {
	// char[][] plan = new char[planner.cells.length][planner.cells[0].length];
	//
	// for (CombinedMove m : moves) {
	// if (plan[m.Robot1Move.y][m.Robot1Move.x] == '2'
	// || plan[m.Robot1Move.y][m.Robot1Move.x] == 'X')
	// plan[m.Robot1Move.y][m.Robot1Move.x] = 'X';
	// else
	// plan[m.Robot1Move.y][m.Robot1Move.x] = '1';
	//
	// if (plan[m.Robot2Move.y][m.Robot2Move.x] == '1'
	// || plan[m.Robot2Move.y][m.Robot2Move.x] == 'X')
	// plan[m.Robot2Move.y][m.Robot2Move.x] = 'X';
	// else
	// plan[m.Robot2Move.y][m.Robot2Move.x] = '2';
	// }
	//
	// for (int y = plan.length - 1; y >= 0; --y) {
	// System.out.print(y);
	// for (int x = plan[0].length - 1; x >= 0; --x) {
	// if (plan[y][x] != '1' && plan[y][x] != '2' && plan[y][x] != 'X')
	// plan[y][x] = ' ';
	// System.out.print(' ');
	// System.out.print(plan[y][x]);
	// }
	// System.out.println();
	// }
	// System.out.print(' ');
	// for (int x = plan[0].length - 1; x >= 0; --x) {
	// System.out.print(' ');
	// System.out.print(x);
	// }
	// System.out.println();
	//
	// }

	private boolean isReplanRequired(Map<RobotInfo, RobotData> robotState) {
		boolean retVal = false;
		for (RobotInfo bot : robots) {
			List<Integer> obstacles = new ArrayList<Integer>();
			RobotData data = robotState.get(bot);

			for (GridPlanner planner : planners.values())
				planner
						.changeCellState(bot.position, CellState.explored,
								false);

			if (RobotData.isObstacle(data.distanceNorth))
				obstacles.add(RobotHeading.NORTH);
			else
				setScannedOpen(planners.get(bot).convertHeadingToCoordinate(
						bot.position, RobotHeading.NORTH));
			if (RobotData.isObstacle(data.distanceEast))
				obstacles.add(RobotHeading.EAST);
			else
				setScannedOpen(planners.get(bot).convertHeadingToCoordinate(
						bot.position, RobotHeading.EAST));
			if (RobotData.isObstacle(data.distanceSouth))
				obstacles.add(RobotHeading.SOUTH);
			else
				setScannedOpen(planners.get(bot).convertHeadingToCoordinate(
						bot.position, RobotHeading.SOUTH));
			if (RobotData.isObstacle(data.distanceWest))
				obstacles.add(RobotHeading.WEST);
			else
				setScannedOpen(planners.get(bot).convertHeadingToCoordinate(
						bot.position, RobotHeading.WEST));

			for (int heading : obstacles) {
				if (isNewObstacle(planners.get(bot).convertHeadingToCoordinate(
						bot.position, heading))) {
					setScannedOccupied(planners.get(bot)
							.convertHeadingToCoordinate(bot.position, heading));
					System.err.println(bot.toString() + " obstacle "
							+ RobotHeading.toString(heading));
					retVal = true;
				}
			}
		}

		if (moves == null || moves.size() == 0)
			retVal = true;
		else {
			CombinedMove lastMove = moves.get(moves.size() - 1);
			if (planners.get(RheinoBot).cells[lastMove.Robot1Move.y][lastMove.Robot1Move.x] == CellState.scannedOpen
					|| planners.get(NXT).cells[lastMove.Robot2Move.y][lastMove.Robot2Move.x] == CellState.scannedOpen)
				retVal = true;
		}

		return retVal;
	}

	private void setScannedOccupied(Point obstacle) {
		if (isInBounds(obstacle))
			for (GridPlanner planner : planners.values())
				planner.changeCellState(obstacle, CellState.scannedOccupied,
						false);
	}

	private boolean isInBounds(Point obstacle) {
		GridPlanner planner = planners.values().iterator().next();
		return obstacle.y >= 0 && obstacle.y < planner.cells.length
				&& obstacle.x >= 0 && obstacle.x < planner.cells[0].length;
	}

	private void setScannedOpen(Point scannedOpen) {
		if (isInBounds(scannedOpen)) {
			if (planners.values().iterator().next().cells[scannedOpen.y][scannedOpen.x] != CellState.scannedOccupied) {
				for (GridPlanner planner : planners.values())
					planner.cells[scannedOpen.y][scannedOpen.x] = CellState.scannedOpen;
			} else {
				for (GridPlanner planner : planners.values())
					System.err
							.println("ERROR: "
									+ scannedOpen
									+ " was "
									+ planner.cells[scannedOpen.y][scannedOpen.x]
									+ '.');
			}
		}
	}

	private boolean isNewObstacle(Point obstacle) {
		if (!isInBounds(obstacle))
			return false;
		GridPlanner planner = planners.values().iterator().next();
		if (planner.cells[obstacle.y][obstacle.x] == CellState.scannedOccupied)
			return false;
		for (RobotInfo bot : robots)
			if (bot.position.equals(obstacle))
				return false;
		return true;
	}

	private Map<RobotInfo, RobotData> sense() {
		Map<RobotInfo, RobotData> robotState = new HashMap<RobotInfo, RobotData>();

		for (RobotInfo bot : robots) {
			RobotData data = rdp.readData(connections.get(bot).getDataIn());
			robotState.put(bot, data);
			if (!data.isErrorFree()) {
				System.err.println("ERROR: " + bot.name
						+ " returned erroneous data.");
				for (GridPlanner planner : planners.values())
					planner.changeCellState(bot.position,
							CellState.scannedOccupied, false);
				robots.remove(bot);
			}
		}

		return robotState.size() == 0 ? null : robotState;
	}
}
